function [v, omega, state_vars] = safe_motion_controller(pose, obstacles, state_vars, v_base, omega_base, dt)
% 统一的安全运动控制器（避障）
% 输入: pose-当前位姿, obstacles-障碍物, state_vars-状态变量, v_base/omega_base-基础速度
% 输出: v/omega-控制量, state_vars-更新的状态变量

    persistent t_counter;
    if isempty(t_counter)
        t_counter = 0;
    end
    t_counter = t_counter + 1;
    
    % 边界
    boundary = struct();
    boundary.x_min = 5;
    boundary.x_max = 45;
    boundary.y_min = 5;
    boundary.y_max = 45;
    boundary.margin = 1.5;
    
    safety_margin = 1.5;
    
    x = pose(1);
    y = pose(2);
    theta = pose(3);
    
    % 障碍物碰撞检测
    [collision, escape_dir] = check_collision(pose, obstacles, safety_margin);
    
    if collision && ~state_vars.obstacle_avoiding
        state_vars.obstacle_avoiding = true;
        state_vars.avoidance_timer = 30;
        target_theta = atan2(escape_dir(2), escape_dir(1));
        state_vars.angle_diff = wrapToPi(target_theta - theta);
        % 限制最大转向角度为120度
        if abs(state_vars.angle_diff) > 2*pi/3
            state_vars.angle_diff = sign(state_vars.angle_diff) * 2*pi/3;
        end
    end
    
    % 边界检测
    near_x_min = (x - boundary.x_min) < boundary.margin;
    near_x_max = (boundary.x_max - x) < boundary.margin;
    near_y_min = (y - boundary.y_min) < boundary.margin;
    near_y_max = (boundary.y_max - y) < boundary.margin;
    at_boundary = near_x_min || near_x_max || near_y_min || near_y_max;
    
    if at_boundary && ~state_vars.boundary_avoiding
        state_vars.boundary_avoiding = true;
        state_vars.avoidance_timer = 50;
        % 平滑转向：选择较小的转向角度
        target_theta = theta;
        if near_x_min
            opt1 = 0;
            opt2 = pi/2;
            if abs(wrapToPi(opt1 - theta)) < abs(wrapToPi(opt2 - theta))
                target_theta = opt1;
            else
                target_theta = opt2;
            end
        end
        if near_x_max
            opt1 = pi;
            opt2 = -pi/2;
            if abs(wrapToPi(opt1 - theta)) < abs(wrapToPi(opt2 - theta))
                target_theta = opt1;
            else
                target_theta = opt2;
            end
        end
        if near_y_min
            opt1 = pi/2;
            opt2 = 0;
            if abs(wrapToPi(opt1 - theta)) < abs(wrapToPi(opt2 - theta))
                target_theta = opt1;
            else
                target_theta = opt2;
            end
        end
        if near_y_max
            opt1 = -pi/2;
            opt2 = pi;
            if abs(wrapToPi(opt1 - theta)) < abs(wrapToPi(opt2 - theta))
                target_theta = opt1;
            else
                target_theta = opt2;
            end
        end
        state_vars.angle_diff = wrapToPi(target_theta - theta);
        % 限制最大转向角度为120度
        if abs(state_vars.angle_diff) > 2*pi/3
            state_vars.angle_diff = sign(state_vars.angle_diff) * 2*pi/3;
        end
        state_vars.obstacle_avoiding = false;
    end
    
    % 生成控制量
    if (state_vars.boundary_avoiding || state_vars.obstacle_avoiding) && state_vars.avoidance_timer > 0
        v = v_base * 0.5;
        % 平滑转向：根据角度差调整转速
        omega = sign(state_vars.angle_diff) * min(abs(state_vars.angle_diff) / dt / 10, 0.3);
        state_vars.avoidance_timer = state_vars.avoidance_timer - 1;
        if state_vars.avoidance_timer == 0
            state_vars.boundary_avoiding = false;
            state_vars.obstacle_avoiding = false;
        end
    else
        % 正常探索：平滑运动，避免大幅转向
        if mod(t_counter, 60) == 0  % 增加调整间隔（从40到60），保持方向稳定性
            rng(t_counter);
            v = v_base * (0.9 + 0.2 * rand());  % 速度更稳定（0.9-1.1倍）
            
            % 平滑角速度调整：基于当前角速度微调，避免突变
            if t_counter == 60
                % 初次设置：小范围随机
                omega = omega_base * (-0.5 + rand());  % -0.5~0.5范围
            else
                % 后续调整：在当前值基础上微调（±30%）
                omega_change = state_vars.omega * (-0.3 + 0.6 * rand());
                omega = state_vars.omega + omega_change;
                % 限制最大角速度，避免急转弯
                omega = max(-0.2, min(0.2, omega));
            end
            
            state_vars.v = v;
            state_vars.omega = omega;
        else
            v = state_vars.v;
            omega = state_vars.omega;
        end
    end
end

function state_vars = init_motion_state()
    state_vars.boundary_avoiding = false;
    state_vars.obstacle_avoiding = false;
    state_vars.avoidance_timer = 0;
    state_vars.angle_diff = 0;
    state_vars.v = 1;
    state_vars.omega = 0;  % 初始角速度为0，直线前进
end

